/*
 * StartApplication.java
 *
 * Created on Jul 19, 2010 12:55:27 PM;
 */

package org.sunspotworld;

import L298NDriver.L298NDriver;
import com.sun.spot.io.j2me.radiogram.Radiogram;
import com.sun.spot.io.j2me.radiogram.RadiogramConnection;
import com.sun.spot.peripheral.ISleepManager;
import com.sun.spot.peripheral.Spot;
import com.sun.spot.peripheral.radio.IRadioPolicyManager;
import com.sun.spot.resources.transducers.IOutputPin;
import com.sun.spot.resources.transducers.ITriColorLED;
import com.sun.spot.resources.transducers.LEDColor;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.EDemoController;
import com.sun.spot.sensorboard.io.PinDescriptor;
import com.sun.spot.service.BootloaderListenerService;
import com.sun.spot.util.Utils;
import java.io.IOException;
import javax.microedition.io.Connector;
import javax.microedition.midlet.MIDlet;
import javax.microedition.midlet.MIDletStateChangeException;

/**
 * The startApp method of this class is called by the VM to start the
 * application.
 * 
 * The manifest specifies this class as MIDlet-1, which means it will
 * be selected for execution.
 */
public class StartApplication extends MIDlet {

    private IOutputPin enableA = EDemoBoard.getInstance().getOutputPins()[EDemoBoard.H0];
    private IOutputPin inputA1 = EDemoBoard.getInstance().getIOPins()[EDemoBoard.D1];
    private IOutputPin inputA2 = EDemoBoard.getInstance().getIOPins()[EDemoBoard.D2];
    private IOutputPin enableB = EDemoBoard.getInstance().getOutputPins()[EDemoBoard.H1];
    private IOutputPin inputB1 = EDemoBoard.getInstance().getIOPins()[EDemoBoard.D3];
    private IOutputPin inputB2 = EDemoBoard.getInstance().getIOPins()[EDemoBoard.D4];
    private IOutputPin logicSupply = EDemoBoard.getInstance().getOutputPins()[EDemoBoard.H3];
    private PinDescriptor enableADescriptor = EDemoController.H0;
    private PinDescriptor enableBDescriptor = EDemoController.H1;

    private ISleepManager sleep = Spot.getInstance().getSleepManager();

    private String port = "51";

    private int x = 1;
    private int y = 1;
    private int throtle = 0;
    private int dutyCycle = 0;

    private boolean receiving = true;

    private static int FORWARD = 2;
    private static int RIGHT = 2;
    private static int STILL = 1;
    private static int REVERSE = 0;
    private static int LEFT = 0;

    private ITriColorLED[] leds = EDemoBoard.getInstance().getLEDs();

    protected void startApp() throws MIDletStateChangeException {
        new BootloaderListenerService().start();   // monitor the USB (if connected) and recognize commands from host

        sleep.disableDeepSleep();

        L298NDriver left = new L298NDriver(enableA,inputA1,inputA2, enableADescriptor);
        L298NDriver right = new L298NDriver(enableB,inputB1,inputB2, enableBDescriptor);

        logicSupply.setHigh();

        IRadioPolicyManager rpm = Spot.getInstance().getRadioPolicyManager();
        rpm.setChannelNumber(25);

        RadiogramConnection rdgConnection = null;
        Radiogram rdg = null;
        try {
            rdgConnection = (RadiogramConnection) Connector.open("radiogram://:"+ port);
            rdg = (Radiogram)rdgConnection.newDatagram(rdgConnection.getMaximumLength());
        } catch(IOException e) {

        }

        while(receiving) {
            for(int i = 0; i < leds.length; i++) {
                leds[i].setOff();
            }
            try {
                rdg.reset();
                rdgConnection.receive(rdg);
                x = rdg.readInt();
                y = rdg.readInt();
                throtle = rdg.readInt();
                dutyCycle = rdg.readInt();
                receiving = true;
            } catch(IOException e) {

            }

            if(y == FORWARD) {

                for(int i = 0; i < leds.length; i++) {
                    leds[i].setColor(LEDColor.GREEN);
                }

                if(x == LEFT) {

                    for(int i = 0; i < (leds.length - 4); i++) {
                        leds[i].setOn();
                    }
                    if(throtle == 1) {
                        ForwardLeft(left, right, dutyCycle);
                    }
                    else {
                        FreeRunning(left);
                        FreeRunning(right);
                    }
                }
                else if(x == RIGHT) {

                    for(int i = 4; i < leds.length; i++) {
                        leds[i].setOn();
                    }
                    if(throtle == 1) {
                        ForwardRight(left, right, dutyCycle);
                    }
                    else {
                        FreeRunning(left);
                        FreeRunning(right);
                    }
                }
                else if(x == STILL) {

                    for(int i = 0; i < leds.length; i++) {
                        leds[i].setOn();
                    }
                    if(throtle == 1) {
                        Forward(left, right, dutyCycle);
                    }
                    else {
                        FreeRunning(left);
                        FreeRunning(right);
                    }
                }
            }
            else if(y == REVERSE) {

                for(int i = 0; i < leds.length; i++) {
                    leds[i].setColor(LEDColor.RED);
                }

                if(x == LEFT) {

                    for(int i = 0; i < (leds.length - 4); i++) {
                        leds[i].setOn();
                    }
                    if(throtle == 1) {
                        ReverseLeft(left, right, dutyCycle);
                    }
                    else {
                        FreeRunning(left);
                        FreeRunning(right);
                    }
                }
                else if(x == RIGHT) {

                    for(int i = 4; i < leds.length; i++) {
                        leds[i].setOn();
                    }
                    if(throtle == 1) {
                        ReverseRight(left, right, dutyCycle);
                    }
                    else {
                        FreeRunning(left);
                        FreeRunning(right);
                    }
                }
                else if(x == STILL) {

                    for(int i = 0; i < leds.length; i++) {
                        leds[i].setOn();
                    }
                    if(throtle == 1) {
                        Reverse(left, right, dutyCycle);
                    }
                    else {
                        FreeRunning(left);
                        FreeRunning(right);
                    }
                }
            }
            else if(y == STILL) {

                for(int i = 0; i < leds.length; i++) {
                    leds[i].setColor(LEDColor.YELLOW);
                }

                if(x == LEFT) {

                    for(int i = 0; i < (leds.length - 4); i++) {
                        leds[i].setOn();
                    }
                    if(throtle == 1) {
                        RotateLeft(left, right, dutyCycle);
                    }
                    else {
                        FreeRunning(left);
                        FreeRunning(right);
                    }
                }
                else if(x == RIGHT) {

                    for(int i = 4; i < leds.length; i++) {
                        leds[i].setOn();
                    }
                    if(throtle == 1) {
                        RotateRight(left, right, dutyCycle);
                    }
                    else {
                        FreeRunning(left);
                        FreeRunning(right);
                    }
                }
                else if(x == STILL) {

                    for(int i = 0; i < leds.length; i++) {
                        leds[i].setOn();
                    }

                    FreeRunning(left);
                    FreeRunning(right);
                }
            }
            else {
                for(int i = 0; i < leds.length; i++) {
                    leds[i].setColor(LEDColor.YELLOW);
                    leds[i].setOn();
                }
                Stop(left);
                Stop(right);
            }
        }

        logicSupply.setLow();

        notifyDestroyed();                      // cause the MIDlet to exit
    }

    public void Forward(L298NDriver left, L298NDriver right, int duty) {
        if(duty > 255) {
            left.DirectionA();
            right.DirectionA();
        }
        else {
            left.PWMDirectionA(duty);
            right.PWMDirectionA(duty);
        }
    }

    public void Reverse(L298NDriver left, L298NDriver right, int duty) {
        if(duty > 255) {
            left.DirectionB();
            right.DirectionB();
        }
        else {
            left.PWMDirectionB(duty);
            right.PWMDirectionB(duty);
        }
    }

    public void ForwardLeft(L298NDriver left, L298NDriver right, int duty) {
        if(duty > 255) {
            right.DirectionA();
            left.PWMDirectionA((256/2));
        }
        else {
            right.PWMDirectionA(duty);
            left.PWMDirectionA((duty/2));
        }
    }
    public void ReverseLeft(L298NDriver left, L298NDriver right, int duty) {
        if(duty > 255) {
            right.DirectionB();
            left.PWMDirectionB((256/2));
        }
        else {
            right.PWMDirectionB(duty);
            left.PWMDirectionB((duty/2));
        }
    }

    public void ForwardRight(L298NDriver left, L298NDriver right, int duty) {
        if(duty > 255) {
            left.DirectionA();
            right.PWMDirectionA((256/2));
        }
        else {
            left.PWMDirectionA(duty);
            right.PWMDirectionA((duty/2));
        }
    }

    public void ReverseRight(L298NDriver left, L298NDriver right, int duty) {
        if(duty > 255) {
            left.DirectionB();
            right.PWMDirectionB((256/2));
        }
        else {
            left.PWMDirectionB(duty);
            right.PWMDirectionB((duty/2));
        }
    }

    public void RotateRight(L298NDriver left, L298NDriver right, int duty) {
        if(duty > 255) {
            left.DirectionA();
            right.DirectionB();
        }
        else {
            left.PWMDirectionA(duty);
            right.PWMDirectionB(duty);
        }
    }

    public void RotateLeft(L298NDriver left, L298NDriver right, int duty) {
        if(duty > 255) {
            left.DirectionB();
            right.DirectionA();
        }
        else {
            left.PWMDirectionB(duty);
            right.PWMDirectionA(duty);
        }
    }

    public void FreeRunning(L298NDriver motor) {
        motor.FreeRunning();
    }

    public void Stop(L298NDriver motor) {
        motor.Stop();
    }

    protected void pauseApp() {
        // This is not currently called by the Squawk VM
    }

    /**
     * Called if the MIDlet is terminated by the system.
     * I.e. if startApp throws any exception other than MIDletStateChangeException,
     * if the isolate running the MIDlet is killed with Isolate.exit(), or
     * if VM.stopVM() is called.
     * 
     * It is not called if MIDlet.notifyDestroyed() was called.
     *
     * @param unconditional If true when this method is called, the MIDlet must
     *    cleanup and release all resources. If false the MIDlet may throw
     *    MIDletStateChangeException  to indicate it does not want to be destroyed
     *    at this time.
     */
    protected void destroyApp(boolean unconditional) throws MIDletStateChangeException {
    }
}
